#include "lineprocessing.h"


QMutex lineProcessing::m; //defined here

lineProcessing::lineProcessing(QString inVideo, QString outYML)
{
    inVideoFile=inVideo;
    outYMLFile=outYML;
    terminateCheck=false; //default value


    //default values
    cutoffB =70; //set to 70% of max for Blue
    cutoffR =70;  //set to 70% of max for Red
    minThresh= 50;     //set a minimum threshold for cutoff
    lineangle=3;        // line angle factor
    filterOnOff=false; // toggle for filtering
    minConfExport=200;  //the minimum confidence for export, anything lower will not be exported
    testLocation=0;  //location in the video for a test frame to be processed
    resultsOutput=true;

}

void lineProcessing::terminateProcessing(){
    //signal sent from master
    terminateCheck=true;
}


int lineProcessing::doIt()
{

    Mat matImage;
    bool eof=false;

    VideoCapture capFile; //Capture object used for reading video file
    FileStorage fs;  //file storage for outputting (YML format)

    bool peakOnOff=false;   // toggle peaks showing
    bool numsOnOff=true;   // toggle laser numbers showing
    bool boostConf=true;         //toggle boost confidence code


    //open the video file
    m.lock();
    capFile.open(inVideoFile.toStdString());
    m.unlock();
    if(!capFile.isOpened())  // make sure we opened
    {
        std::cout << "Unable to open input video file" << inVideoFile.toStdString()<<"\n" ;
        emit finished();
        return -1;
    }


    if (resultsOutput){
        //open the output file
        fs.open(outYMLFile.toStdString(), FileStorage::WRITE);  //open for writing new file
        if(!fs.isOpened()) {
            std::cout << "Unable to open output YML file" << outYMLFile.toStdString()<<"\n" ;
            emit finished();
            return -1;
        }
        fs <<"Frames"<<"["; // initialize sequence
    }

    int frameTotal=capFile.get(CV_CAP_PROP_FRAME_COUNT); //length of video file in frames

    // run through the video file frame-by-frame
    do {
        // check for end of file
        int curPos=capFile.get(CV_CAP_PROP_POS_FRAMES)+1; //add one because function rval is zero based

        if(curPos>frameTotal)  //check if we reached the end of the video
            eof=true;
        else
            capFile.read(matImage);

         if (!resultsOutput) { //if not writing resuts, this is a test run

             // THIS IS NOT WORKING.  CAN'T FIGURE OUT WHY
             //double vidLocation=testLocation / 100.0;
             //capFile.set(CV_CAP_PROP_POS_AVI_RATIO, vidLocation); //move to the frame we want to test

             int vidLocation= testLocation * frameTotal /100;
             capFile.set(CV_CAP_PROP_POS_FRAMES, vidLocation); //move to the frame we want to test


             capFile >> matImage; //grab it
         }

        if(filterOnOff) {
            //medianBlur(matImage, matImage, 3);
            GaussianBlur(matImage, matImage, Size(5,5),3); //blur the original
        }

        // allocate array to store the data on the line scan
        //  I'm just using twice the width of the image, which should be OK for size
        // NOTE TO SELF- these should be converted to variable length vectors, instead of being fixed arrays
        int arrayLen=matImage.cols*2;
        unsigned char* charArrayB = new unsigned char[arrayLen];   // Blue Channel
        unsigned char* charArrayG = new unsigned char[arrayLen];   // Green Channel
        unsigned char* charArrayR = new unsigned char[arrayLen];   // Red Channel
        unsigned int*   intArrayX = new unsigned int[arrayLen];    // x values in the image
        unsigned int*   intArrayY = new unsigned int[arrayLen];    // y values in the image

        int spacing=1;  //not used anymore, was for debugging.  always set to one

        // create a mat image to store the peaks in
        Mat matPeaks(matImage.size(),CV_8UC(3),Scalar::all(0));

        // create a mat (non-image) to store the Line Numbers & Confidence data
        Mat matLines(matImage.size(),CV_8UC(7),Scalar::all(0));   // three scan line passes, plus four confidence numbers

        Mat_< Vec<uchar, 7> > _matLines = matLines;               //pointer access to the data, for easy notation


        // three-scan
        for(int k=0;k<3;k++) {
            int angleDir=0;  //the angle direction. Zero: straight up, first pass
            if(k == 1) angleDir=1; //forward angle, second pass
            if(k == 2) angleDir=-1; //back angle, third pass
            //if(k == 2) angleDir=2; //second forward angle

            //start line cross-scan
            //initial scan line variables
            int ystart=matImage.rows;  //Y-starting point for the scan, bottom of image
            int xstart=0; //X-starting point for the scan

            for(int j=0;j<matImage.cols/spacing;j++) {

                xstart+=spacing; //advance to the next scan line


                int vals;  //keep track of how many pixels we find
                char blueMax=0; // keep track of max blue value
                char redMax=0;  // keep track of max red value

                //iterate along the line populating the values
                for(int i=0;i<matImage.rows;i++) {

                    int xloc=xstart+(angleDir*i)/(lineangle);
                    int yloc=-i+ystart-1;

                    if (xloc<0)break; if(xloc>=matImage.cols)break;
                    if (yloc<0)break; if(yloc>=matImage.rows) break;


                    // save the XY position in the image
                    intArrayX[i]=xloc;
                    intArrayY[i]=yloc;

                    charArrayB[i]= matImage.at<cv::Vec3b>(yloc,xloc)[0]; //grab the first byte (blue channel)
                    charArrayG[i]= matImage.at<cv::Vec3b>(yloc,xloc)[1]; //grab the second byte (green channel)
                    charArrayR[i]= matImage.at<cv::Vec3b>(yloc,xloc)[2]; //grab the third byte (red channel)

                    //update max vals
                    if(charArrayB[i]>blueMax) blueMax=charArrayB[i];
                    if(charArrayR[i]>redMax) redMax=charArrayR[i];

                      vals=i;

                }


                unsigned int* intArrayPeaks = new unsigned int[arrayLen];   //  the position of Generic Peaks on the scan line

                int peakCounter =0; //counter for generic Peaks

                //make a max value array
                unsigned char* charArrayMax = new unsigned char[arrayLen];   // Max Channel
                for(int i=0;i<vals;i++){
                    (charArrayB[i]>charArrayR[i]) ? charArrayMax[i]=charArrayB[i]:charArrayMax[i]=charArrayR[i];
                }

                cutoffB = cutoffB*blueMax/100; //Scaled to the max magnitude of blue found (orig. val. is percent so divide by 100)
                cutoffR = cutoffR*redMax/100;  //Scaled to the max magnitude of red found (orig. val. is percent so divide by 100)

                for(int i=2;i<vals-2;i++) {  //iterate through the arrays finding peaks (from start +2, to end-2)
                    //count peaks
                    if(charArrayMax[i]>minThresh) //if above absolute floor
                        if(charArrayMax[i]>cutoffB || charArrayMax[i]>cutoffR){  //if above cutoff  NOTE TO SELF: this has almost no effect.  I need to separate Red and Blue here if I want this to work as intended.

                            if(charArrayMax[i] > charArrayMax[i-1] && charArrayMax[i] >= charArrayMax[i+1] &&
                                    charArrayMax[i] > charArrayMax[i-2] && charArrayMax[i] >= charArrayMax[i+2]) {
                                intArrayPeaks[peakCounter]=i;   // add it to the peak list
                                peakCounter++;                  // increment the generic counter
                            }

                        }
                }


                //populate the peak color sequence (Red-Blue)
                // check for out of sequence BRBRB...
                unsigned char* peakSeq = new unsigned char[peakCounter];
                int oosCount=0;  //zero the Out of Sequence counter
                for(int i=0;i<peakCounter;i++){
                    if(charArrayR[intArrayPeaks[i]] > charArrayB[intArrayPeaks[i]]) {
                        peakSeq[i]='R';
                    }
                    else {
                        peakSeq[i]='B';
                    }
                    if(i>0)  // make sure we are past the first one
                        if( peakSeq[i] == peakSeq[i-1])oosCount++; //if we repeat add to the counter
                }


                //draw a dot at the peak
                if(peakOnOff)
                    for(int i=0;i<peakCounter;i++){
                        int xloc=intArrayX[intArrayPeaks[i]];
                        int yloc=intArrayY[intArrayPeaks[i]];

                        //draw in to the peak image
                        circle(matPeaks,Point(xloc,yloc),
                               0,Scalar(255*(peakSeq[i]=='B'),0,255*(peakSeq[i]=='R')),-1);
                    }


                //Line number confidence method starts here

                int blueCount=0; // initialize red & blue counters
                int redCount=0;

                for(int i=0;i<peakCounter;i++) { //iterate through the peak list checking order
                    int x =intArrayX[intArrayPeaks[i]]; //col
                    int y =intArrayY[intArrayPeaks[i]]; //row
                    if(peakSeq[i]=='B')
                        if(blueCount<=redCount)
                            _matLines(y,x)[k]= blueCount=redCount+1; //line number assigned, current red+1
                        else
                            _matLines(y,x)[k]= blueCount+=2; //there is a blue missing so add two to current blue
                    else {
                        if(redCount<blueCount)
                            _matLines(y,x)[k]= redCount=blueCount+1; //line number assigned, current blue+1
                        else
                            _matLines(y,x)[k]= redCount+=2; //there is a blue missing so add two to current red
                    }
                }

                 //Confidence Calcs
                int peakDelta=abs(14-peakCounter);
                char peakConf=1;
                char oosConf=1;
                switch (peakDelta) {
                case 0: peakConf=100;    // all 14 lines visible
                    break;
                case 1: peakConf=60;    // one more/less line
                    break;
                case 2: peakConf=40;    // two more/less lines
                    break;
                case 3: peakConf=20;    // three more/less lines
                    break;
                case 4: peakConf=10;    // four more/less lines

                }
                if(peakConf)  //not sure why I added this check
                    switch (oosCount) {
                    case 0:  oosConf=100;    // none out of seq
                        break;
                    case 1:  oosConf=50;    // one out of seq
                        break;
                    case 2:  oosConf=10;    // two out of seq

                    }

                //iterate and assign confidence from above calcs
                for(int i=0;i<peakCounter;i++){
                    int xloc=intArrayX[intArrayPeaks[i]];
                    int yloc=intArrayY[intArrayPeaks[i]];

                    _matLines(yloc,xloc)[k+3]=peakConf+oosConf;  // CH3-5 is confidence level
                    _matLines(yloc,xloc)[6] +=85;                // CH6 is hit count
                    //  one hit = 85, two = 170, all three = 255
                }


                delete[] intArrayPeaks;
                delete[] charArrayMax;
                delete[] peakSeq;


            }  //end line cross scan

        } //end three-scan


        // find the best line number for each pixel in the image
        for(int j=0;j<matImage.cols;j++) {
            for(int i=0; i<matImage.rows;i++) {
                int x =j; //col
                int y =i; //row
                bestLine(matLines, x, y);  //call best line function (modifies matLines)
            }
        }

        // Boost confidence
        if(boostConf){
            //start at the center bottom and work upward & outward
            int ystart=matImage.rows;  //Y-starting point for the scan, bottom of image
            int xstart=matImage.cols/2; //X-starting point for the scan, middle
            int flipper=1;

            for (int j=0;j<matImage.cols/2;j++){
                for(int i=0;i<matImage.rows;i++) {

                    int xloc=xstart+flipper*j;  // working outward, oscillating left/right
                    int yloc=ystart-3-i; //offset three to be away from edge

                    //bounds check.  make sure we don't go off the image
                    if (xloc<2)break; if(xloc>=matImage.cols-2)break;
                    if (yloc<2)break; if(yloc>=matImage.rows-2) break;

                    if(_matLines(yloc,xloc)[6])
                        checkNeighbor(matLines,matPeaks,xloc,yloc);  //call check neighbor function

                    if(flipper==1)flipper=-1;
                    else flipper=1;
                }
            }
        } //End boost confidence

        //Numbering
        if(numsOnOff)
        {
            //char text[5];

            // iterate through the image and print the "estmated" laser line number
            int numberCount=0;
            for(int j=0;j<matImage.cols;j+=1) {
                if(numberCount++ >15)numberCount=0;

                for(int i=0; i<matImage.rows;i++) {
                    int x =j; //col
                    int y =i; //row

                    if( _matLines(y,x)[6]) {

                        uchar conf=_matLines(y,x)[5]; //confidence at this point
                        //put a dot on it
                        if(conf>minConfExport)
                            circle(matPeaks,Point(x,y),0,Scalar(0,255,0),-1);  //will be exported, GREEN
                        if(conf<=minConfExport && conf>minConfExport*9/10)
                            circle(matPeaks,Point(x,y),0,Scalar(0,255,255),-1); //wont be exported, its 90% or better, YELLOW
                        if(conf<=minConfExport*9/10 && conf>minConfExport*8/10)
                            circle(matPeaks,Point(x,y),0,Scalar(0,165,255),-1); //wont be exported, its 80% or better, ORANGE
                        if(conf<minConfExport*8/10)
                            circle(matPeaks,Point(x,y),0,Scalar(0,0,255),-1); //wont be exported, its less than 80%, RED

                        if (numberCount > 15) {//skip 15 then print


                            CvScalar clr=Scalar(0,255,0); //set our color based on confidence, default Green
                            if(conf<minConfExport)
                                clr=Scalar(0,255,255); // paint Yellow
                            if(conf<minConfExport*9/10)
                                clr=Scalar(0,165,255); // paint Orange
                            if(conf<minConfExport*8/10)
                                clr=Scalar(0,0,255); // paint Red                }

                            QString text=QString::number(_matLines(y,x)[6]);
                            putText(matPeaks, text.toStdString(), Point(x,y), FONT_HERSHEY_PLAIN, 1,clr);
                        }
                    }

                }
            }
        } //End Numbering

        //Display
//        imshow("peaks",matPeaks);
//        char c=waitKey(100);
//        if(c==27) return -1;  //bail out on escape key

        //free dynamically allocated memory
        delete[] charArrayB;
        delete[] charArrayG;
        delete[] charArrayR;
        delete[] intArrayX;
        delete[] intArrayY;

        if (resultsOutput){
            //write high confidence data points to file
            fs << "{";
            writePoints(matLines,minConfExport,capFile.get(CV_CAP_PROP_POS_FRAMES),fs);
            fs << "}";
        }

        //send the peak image back to the dialog
        cvtColor(matPeaks,matPeaks,CV_BGR2RGB);
        qimgLineImage = QImage((uchar*)matPeaks.data,matPeaks.cols,matPeaks.rows,
                               matPeaks.step,QImage::Format_RGB888).copy();
        emit lineImage(); //send signal for image ready for display

        emit status(curPos*100/frameTotal); //send a signal to tell percent complete

        if(terminateCheck) break; //we recieved a signal to terminate, exit do loop and close the files

        if(!resultsOutput) break; //if we are not writing results, this is a test run, so break after processing one frame.

    } while(!eof); // Go until end of file was reached



     if (resultsOutput){
        //close out the file
        fs <<"]";
        fs.release();
    }

    m.lock();
    capFile.release();
    m.unlock();

    emit finished();

    terminateCheck=false; //reset this for future calls
    resultsOutput=true; //reset this for future calls

    return 0; //No errors
}

////////////////////////////////////////////////////////////////////////////////////////////
uchar lineProcessing::bestLine(Mat &matL,int x, int y) {
    // returns the best line number from the three-scan process
    //  NOTE:  this function changes matL fields 5 & 6

    Mat_< Vec<uchar, 7> > _matL = matL;    //pointer access to the data, for easy notation

    uchar scan0L=_matL(y,x)[3];        //confidence score of scan 0
    uchar scan1L=_matL(y,x)[4];        //confidence score of scan 1
    uchar scan2L=_matL(y,x)[5];        //confidence score of scan 2
    uchar scan3L=_matL(y,x)[6];        //hit counter

    uchar rVal=0;                       // set to zero

    if (scan3L>85) {          //  if the hit counter has at least two hits
        if( scan2L > scan1L ) {           //if scan 2 is higher conf than scan 1
            if( scan2L > scan0L) {             //and if scan 2 is higher conf than scan 0
                rVal= _matL(y,x)[2];             // then return line number of scan 2
                //_matL(y,x)[5]=scan2L;       // conf value is already in field 5, no need to change
            }
            else {
                rVal= _matL(y,x)[0];             // then return line number of scan 0
                _matL(y,x)[5]=scan0L;        // store confidence in field 5
            }
        }
        else {
            if(scan1L > scan0L) {          //if scan 1 is higher than scan 0
                rVal= _matL(y,x)[1];             // then return line number of scan 1
                _matL(y,x)[5]=scan1L;           // store confidence in field 5
            }
            else
                rVal=_matL(y,x)[0];        //else return line number of scan 0
            _matL(y,x)[5]=scan0L;        // store confidence in field 5
        }

        //do all three scan line numbers match?
        if(scan3L>170) {// if hit counter is all three
            if( (_matL(y,x)[0]==_matL(y,x)[1]) && (_matL(y,x)[0]==_matL(y,x)[2]) ) {
                char delta = 255-_matL(y,x)[5]; //difference between our current confidence and a perfect score of 255
                _matL(y,x)[5] += (delta/4)*3; //add 3/4 the difference to our confidence

            }
        }
        else { //do two line numbers match, and are the the chosen number  NOTE TO SELF: I might want to do this at the very top of this function
            if(        (_matL(y,x)[0]==_matL(y,x)[1] && _matL(y,x)[0]==rVal)
                    || (_matL(y,x)[1]==_matL(y,x)[2] && _matL(y,x)[0]==rVal)
                    || (_matL(y,x)[2]==_matL(y,x)[0] && _matL(y,x)[0]==rVal)
               ) {
                char delta = 255-_matL(y,x)[5]; //difference between our current confidence and a perfect score of 255
                _matL(y,x)[5] += delta/2; //add a 1/2 of the difference to our confidence
            }
        }




    }


    _matL(y,x)[6]=rVal;           //Update the matL, Store the "best" line number in field 6
    return rVal;
}




////////////////////////////////////////////////////////////////////////////////////////////
void lineProcessing::checkNeighbor(Mat &matL,Mat &matP,int x,int y) {
    // NOTE: this function changes matL field 0, 1, 2, 5, and 6
    //    matL: lines
    //    matP: color
    //    x, y: pixel location

    Mat_< Vec<uchar, 7> > _matL = matL;    //pointer access to the data, for easy notation
    Mat_< Vec<uchar, 3> > _matP = matP;    //pointer access to the data, for easy notation

    //debug
    int sent=0;

    //first make sure we have not been here before
    if(_matL(y,x)[1] == 255) {
        return;  //here we are re-using field 1 of the line matrix
    }
    _matL(y,x)[1] = 255;  //now mark that we have been here.


    //scan my neighbors
    for(int i=-2;i<3;i++) {
        for(int j=-2;j<3;j++) {
            if(!(i==0 && j==0)) {      // test to skip myself
                int hisx=x+i;
                int hisy=y+j;

                if(_matL(hisy,hisx)[6])// &&  //make sure he has a pixel
                    //_matL(hisy,hisx)[0] !=255)  //not been changed already.

                    //compare my color to his, if same
                    if( _matP(y,x)[0]==_matP(hisy,hisx)[0] || (_matP(y,x)[2]==_matP(hisy,hisx)[2]) ) { //both Blue or both Red

                        if(_matL(y,x)[6] != _matL(hisy,hisx)[6] ) {   //is his number different than mine?
                            if(_matL(y,x)[5] > _matL(hisy,hisx)[5]) {//is my confidence is higher?
                                _matL(hisy,hisx)[6] = _matL(y,x)[6]; //change his line number to mine
                                _matL(hisy,hisx)[0]=255;   //mark channel 0 so we know we changed it.
                                _matL(hisy,hisx)[5]=(_matL(y,x)[5] + _matL(hisy,hisx)[5])/2; //bump his confidence
//                                //bounds check.  make sure we don't go off the image
//                                int bounds=( (hisx>2) && (hisx<matP.cols-2) &&
//                                             (hisy>2) && (hisy<matP.rows-2) );  //equal true if we are inbounds
//                                if(bounds) {checkNeighbor(matL,matP,hisx,hisy);  //recursively call this function, with his x,y

//                                //debug
//                                sent++;
//                                }

                            } else {
                                _matL(y,x)[6]=_matL(hisy,hisx)[6]; //change my line number to his
                                _matL(y,x)[5]=(_matL(y,x)[5] + _matL(hisy,hisx)[5])/2;  //make my confidence the average of ours
                            }
                            //bounds check, then follow
                            int bounds=( (hisx>2) && (hisx<matP.cols-2) &&
                                         (hisy>2) && (hisy<matP.rows-2) );  //equal true if we are inbounds
                            if(bounds && _matL(hisy,hisx)[1] != 255) {
                                checkNeighbor(matL,matP,hisx,hisy);  //recursively call this function, with his x,y
                                //debug
                                sent++;
                            }




                        }
                        else { // same line number

                            if(_matL(y,x)[5] > _matL(hisy,hisx)[5]) {//check if my confidence is higher
                                _matL(hisy,hisx)[2]=255;   //mark channel 2 so we know we changed it.
                                //_matL(hisy,hisx)[5]=(_matL(y,x)[5] - _matL(hisy,hisx)[5])/2 + 1; //bump his confidence
                                _matL(hisy,hisx)[5]= _matL(y,x)[5]; //bump his confidence to same as mine
                            } else _matL(y,x)[5]=_matL(hisy,hisx)[5];//bump my confidence to same as his

                            //bounds check, then follow
                            int bounds=( (hisx>2) && (hisx<matP.cols-2) &&
                                         (hisy>2) && (hisy<matP.rows-2) );  //equal true if we are inbounds
                            if(bounds && _matL(hisy,hisx)[1] != 255) {
//                                _matL(hisy,hisx)[1] = 255;  // leave a breadcrum
                                checkNeighbor(matL,matP,hisx,hisy);  //recursively call this function, with his x,y
//                                _matL(hisy,hisx)[1] = 0;  // pickup breadcrum

                            //debug
                            sent++;
                            }

                        } // end same line number
                    } //end color check
            } //end test for skipping self
        } //end j-loop
    }// end i-loop

    return;
}

////////////////////////////////////////////////////////////////////////////////////////////

void lineProcessing::writePoints(Mat matL,int minConfidence, int frameNumber, FileStorage& fs) {
    // NOTE: this function outputs the laser line number & x,y location for each pixel peak
    //    matL: lines
    //      field 5: confidence
    //      field 6: line number
    //    minConfidence: Minimum Confidence for export

    Mat_< Vec<uchar, 7> > _matL = matL;    //pointer access to the data, for easy notation

    fs << "FrameNumber" << frameNumber;

    //create a matrix to hold the points to export
    Mat_<Vec3i> exportPoints;  //line number, y-position, x-position
    exportPoints.reserve(14*(matL.rows+matL.cols)); //allocate a block of memory

    for(int i=0;i<matL.rows;i++) {
        for(int j=0;j<matL.cols;j++) {
            if(_matL(i,j)[5]>minConfidence) {//if confidence is high enough
                int lineNumber=_matL(i,j)[6]; // its number
                exportPoints.push_back(Vec3i(lineNumber,i,j));
            }

        }
    }

    fs << "ExportedPoints" << exportPoints;

    return;

}

